#pragma warning (disable : 4251 4996 4819 4244)

#include <libplayerc++/playerc++.h>
#include "ompl/util/RandomNumbers.h"
#include "Person.h"

Person::Person( PlayerCc::Position2dProxy& refMotion, PlayerCc::PlayerClient& refEnv ) :
m_refMotion( refMotion ),
m_refEnv( refEnv )
{
}

Person::~Person(void)
{
}

void Person::RandomWalkStep()
{
    m_refEnv.Read();
    ompl::RNG rngRand;
    double dPosX = m_refMotion.GetXPos() + rngRand.uniformReal( -1.0, 1.0 );
    double dPosY = m_refMotion.GetYPos() + rngRand.uniformReal( -1.0, 1.0 );

    m_refMotion.GoTo( dPosX, dPosY, 0.0 );

    while ( true )
    {
        m_refEnv.Read();
        double dDis = fabs(dPosX - m_refMotion.GetXPos()) + fabs(dPosY - m_refMotion.GetYPos());
        if ( dDis < 0.05 )
        {
            break;
        }
    }
}
